﻿using System.Text;
using TrackSystem.IO.Data;
using TrackSystem.IO.Models;

namespace TrackSystem.IO.Protocol
{
    /// <summary>
    /// 中船重工的惯性导航系统的通信协议类
    /// 寄存器  只读
    /// 0 - 航向
    /// 1 - 俯仰
    /// 2 - 横滚
    /// 3 - 水平角速率
    /// 4 - 俯仰角速率
    /// 5 - 横滚角速率
    /// 6 - 纬度
    /// 7 - 经度
    /// 8 - 高度
    /// 9 - 前向速度
    /// 10 - 右相速度
    /// 11 - 垂直速度
    /// 12 - 自检状态
    /// 13 - PPP 航向角误差标准值
    /// 14 - 惯导状态 0 准备 1 码头对准  2 纯惯性 3 INS+GPS 4 海上对准 5 INS+DVL 7 罗经对准 8 零速修正 9 罗经导航 11 DVL自动标定 12 DVL标定成功
    /// 15 - 惯导时间  hhmmss.ss
    /// 16 - 惯导故障状态 0 - 正常 1 - 故障 2 - 系统故障
    /// 17 - 前向加速度
    /// 18 - 右向加速度
    /// 19 - 垂向加速度
    /// 20 - -
    /// 21 - DVL对底前向加速度(mm/s)
    /// 22 - DVL对底右向加速度(mm/s)
    /// 23 - DVL对底垂向加速度(mm/s)
    /// 24 - DVL对底距底距离（M）
    /// 25 - DVL对底数据质量
    /// 26 - DVL对水前向加速度(mm/s)
    /// 27 - DVL对水右向加速度(mm/s)
    /// 28 - DVL对水垂向加速度(mm/s)
    ///  29 - DVL对水距底距离（M）
    /// 30 - DVL对水数据质量
    /// 
    /// 寄存器 读写
    /// 32 - 命令控制  码头校准命令
    /// 33 - 命令控制  船上校准命令
    /// 34 - 设定经度
    /// 35 - 设定纬度
    /// 36 - 设定海拔高度
    /// 37 - 设定地面速率
    /// 38 - 设定地面航向
    /// </summary>
    public class HskINSProtocol : ProtocolBase
    {
        private bool bSendR;
        private int startTicks;
        private bool IsSeaCal;      // 海上校准
        private bool IsFixPos;

        /// <summary>
        /// 初始状态  0 - 未通信  1-第一次成功通信 
        /// </summary>
        private int initState;
        private int INSState;

        private bool bFirst;
        private int bFirstCount;
        private int counter;


        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="name"></param>
        public HskINSProtocol(string name)
            : base()
        {
            Name = name;
            bSendR = false;
            initState = 1;
            INSState = 0;
            IsAscii = true;
            NeedInit = false;
            IsSeaCal = false;
            IsFixPos = false;

            // 首次进入
            bFirst = true;
            bFirstCount = 0;
            counter = 0;
        }

        /// <summary>
        /// 生成读取寄存器的通讯帧
        /// </summary>
        /// <param name="NeedRecvLen"></param>
        /// <returns></returns>
        public override byte[] GenerateReadFrame(out int NeedRecvLen)
        {
            var DataFactory = RegFactory.Instance;

            // 首次上电进入，延时600次（1分钟）再读取数据
            if (bFirst || bFirstCount < 600) {
                bFirst = false;
                bFirstCount++;
                NeedRecvLen = 0;
                return null;
            }

            // 上次的发送没有收到正确回复，bSend没有被置位
            startTicks = Environment.TickCount;

            // 上次的发送没有收到正确回复，bSend没有被置位
            if (bSendR)
            {
                Fail_R++;
                // 失败超过30次，成功次数清零
                if (Fail_R >= 30)
                {
                    Succ_R = 0;
                }
            }
            bSendR = true;

            // 接收数据长度定义
            NeedRecvLen = 40;

            // 发送位置校正信息  GPS校准
            if (IsSeaCal && (counter++ % 10) == 0)
            {
                double fLon = DataFactory.Regs[RegNameList[34]].dVal;
                double fLat = DataFactory.Regs[RegNameList[35]].dVal;

                int nLatNum = (int)fLat;
                double dLatDeg = nLatNum * 100 + (fLat - nLatNum) * 60;
                int nLonNum = (int)fLon;
                double dLonDeg = nLonNum * 100 + (fLon - nLonNum) * 60;
                // GPS定位协议帧
                // $GPGGA, <UTC时间>, <纬度>, <N|S>, <经度>, <E|W>, <GPS状态>,
                // <卫星数量>, <HDOP水平因子>, <海拔高度>, <地球椭球面相对达到水准面高度>,
                // <差分时间>, <差分站ID号>
                string sRX_temp1 = $"GPGGA,{DateTime.Now:HHmmss},{dLatDeg:0.0000},N,{dLonDeg:0.0000},E,2,10,1.5,{DataFactory.Regs[RegNameList[36]]:0.0}," +
                    $"{DataFactory.Regs[RegNameList[24]]:0.0},1,1";
                byte checksum = 0;
                foreach (byte x in sRX_temp1)
                {
                    checksum ^= x;
                }
                string sRX_Frame1 = $"${sRX_temp1}*{checksum:X2}\r\n";

                // 地面速度信息
                // $GPRMC, <UTC时间>, <定位状态>, <纬度>, <N|S>, <经度>, <E|W>,
                // <地面速率>, <地面航向>, <UTC日期>, <磁偏角>, <磁偏角方向E|W>, <模式指示A|D|E>
                string sTime = $"{DateTime.Now.Hour:00}{DateTime.Now.Minute:00}{DateTime.Now.Second:00}.{DateTime.Now.Millisecond:000}";
                string sDate = $"{DateTime.Now.Day:00}{DateTime.Now.Month:00}{DateTime.Now.Year-2000:00}";
                string sRx_temp2 = $"GPRMC,{sTime},A,{dLatDeg:0.0000},N,{dLonDeg:0.0000},E,{DataFactory.Regs[RegNameList[37]]:0.0}," +
                    $"{DataFactory.Regs[RegNameList[38]]:0.0},{sDate},000.0,E,D";
                checksum = 0;
                foreach (byte x in sRx_temp2)
                {
                    checksum ^= x;
                }
                string sRX_Frame2 = $"${sRx_temp2}*{checksum:X2}\r\n";

                byte[] bRx_Frame = Encoding.UTF8.GetBytes(sRX_Frame1 + sRX_Frame2);
                Tx_r = bRx_Frame;
                return bRx_Frame;
            }


            // 位置校正
            if (IsFixPos && (counter++ % 10) == 0)
            {
                double fLon = DataFactory.Regs[RegNameList[34]].dVal;
                double fLat = DataFactory.Regs[RegNameList[35]].dVal;

                string sRX_temp = $"PIXSE,SETPOS,1,{fLat:0.000000},{fLon:0.000000}";
                byte[] bRx_temp = Encoding.UTF8.GetBytes(sRX_temp);
                byte checksum = 0;
                foreach (byte x in bRx_temp)
                {
                    checksum ^= x;
                }
                string sRX_Frame = $"${sRX_temp}*{checksum:X2}\r\n";
                byte[] bRx_Frame = Encoding.UTF8.GetBytes(sRX_Frame);
                Tx_r = bRx_Frame;
                return bRx_Frame;
            }

            Tx_r = null;
            return null;
        }

        /// <summary>
        /// 处理读取的数据
        /// </summary>
        /// <param name="buffer"></param>
        /// <returns></returns>
        public override bool ProcessReadFrame(byte[] buffer)
        {
            var DataFactory = RegFactory.Instance;

            Lapse = Environment.TickCount - startTicks;
            Succ_R++;
            // 失败次数清零
            if (Succ_R > 0 && Fail_R>0) {
                Fail_R = 0;
            }
            bSendR = false;
            try
            {
                string fmCodes = Encoding.ASCII.GetString(buffer);
                string[] fmLines = fmCodes.Split('\n');
                foreach (var line in fmLines)
                {
                    string[] fmItems = line.Split(',');
                    int fmLen = fmItems.Length;

                    if (fmLen == 0)   // 数据长度为0
                    {
                        continue;
                    }

                    Rx_r = buffer;  // 用于显示的接收帧内容

                    // ---  $PIXSE, HEAVE_, nnnn, nnnn, nnn*hh
                    if (fmLen == 5 && fmItems[0] == "$PIXSE" && fmItems[1] == "HEAVE_")
                    {
                        if (double.TryParse(fmItems[2], out double d1))
                        {
                            DataFactory.SetVal(RegNameList[9], d1);
                        }
                        if (double.TryParse(fmItems[3], out double d2))
                        {
                            DataFactory.SetVal(RegNameList[10], d2);
                        }
                        int pos = fmItems[4].IndexOf('*');
                        if (pos == -1) {
                            continue;
                        }
                        else if (double.TryParse(fmItems[4].Substring(0, pos - 1), out double d3))
                        {
                            DataFactory.SetVal(RegNameList[11], d3);
                        }
                        continue;
                    }

                    // ---  $PIXSE, POSITI, nnnn, nnnn, nnn*hh
                    if (fmLen == 5 && fmItems[0] == "$PIXSE" && fmItems[1] == "POSITI")
                    {
                        if (double.TryParse(fmItems[2], out double d1))
                        {
                            DataFactory.SetVal(RegNameList[6], d1);
                        }
                        if (double.TryParse(fmItems[3], out double d2))
                        {
                            DataFactory.SetVal(RegNameList[7], d2);
                        }
                        int pos = fmItems[4].IndexOf('*');
                        if (pos == -1)
                        {
                            continue;
                        }
                        else if (double.TryParse(fmItems[4].Substring(0, pos - 1), out double d3))
                        {
                            DataFactory.SetVal(RegNameList[8], d3);
                        }
                        continue;
                    }

                    // ---  $PIXSE, ATITUD, nnnn, mmm*hh
                    if (fmLen == 4 && fmItems[0] == "$PIXSE" && fmItems[1] == "ATITUD")
                    {
                        if (double.TryParse(fmItems[2], out double d1))
                        {
                            DataFactory.SetVal(RegNameList[1], d1);
                        }
                        int pos = fmItems[3].IndexOf('*');
                        if (pos == -1)
                        {
                            continue;
                        }
                        else if (double.TryParse(fmItems[3].Substring(0, pos - 1), out double d3))
                        {
                            DataFactory.SetVal(RegNameList[2], d3);
                        }
                        continue;
                    }

                    // ----- $HEHDT, NNNN, T*HH ------------
                    if (fmLen == 3 && fmItems[0] == "$HEHDT")
                    {
                        if (double.TryParse(fmItems[1], out double d1))
                        {
                            DataFactory.SetVal(RegNameList[0], d1);
                        }
                        continue;
                    }

                    // ---  $PHROT, xxxx, yyyy, zzzz*hh
                    if (fmLen == 4 && fmItems[0] == "$PHROT")
                    {
                        if (double.TryParse(fmItems[1], out double d1))
                        {
                            DataFactory.SetVal(RegNameList[3], d1);
                        }
                        if (double.TryParse(fmItems[2], out double d2))
                        {
                            DataFactory.SetVal(RegNameList[4], d2);
                        }
                        int pos = fmItems[3].IndexOf('*');
                        if (pos == -1)
                        {
                            continue;
                        }
                        else if (double.TryParse(fmItems[3].Substring(0, pos - 1), out double d3))
                        {
                            DataFactory.SetVal(RegNameList[5], d3);
                        }
                        continue;
                    }

                    // ---  $PIXSE, IMU, xxxx, yyyy, zzzz, PPP*hh
                    if (fmLen == 6 && fmItems[0] == "$PIXSE" && fmItems[1] == "IMU")
                    {
                        if (double.TryParse(fmItems[2], out double d1))
                        {
                            DataFactory.SetVal(RegNameList[17], d1);
                        }
                        if (double.TryParse(fmItems[3], out double d2))
                        {
                            DataFactory.SetVal(RegNameList[18], d2);
                        }
                        if (double.TryParse(fmItems[4], out double d3))
                        {
                            DataFactory.SetVal(RegNameList[19], d3);
                        }
                        int pos = fmItems[5].IndexOf('*');
                        if (pos == -1)
                        {
                            continue;
                        }
                        else if (double.TryParse(fmItems[5].Substring(0, pos - 1), out double d4))
                        {
                            DataFactory.SetVal(RegNameList[13], d4);
                        }
                        continue;
                    }

                    // ---  $PIXSE, STATE, hhmmss.ss, cc, s*hh
                    if (fmLen == 5 && fmItems[0] == "$PIXSE" && fmItems[1] == "STATE")
                    {
                        double d1 = double.Parse(fmItems[2]);
                        double d2 = double.Parse(fmItems[3]);
                        int d3 = 0;
                        if (fmItems[4].Length > 0 && fmItems[4][0] == 'A')
                            d3 = 1;
                        else if (fmItems[4].Length > 0 && fmItems[4][0] == 'V')
                            d3 = 2;

                        DataFactory.SetVal(RegNameList[15], d1);
                        DataFactory.SetVal(RegNameList[14], d2);
                        DataFactory.SetVal(RegNameList[16], d3);

                        INSState = (int)d2; // 系统状态
                        continue;
                    }

                    // ---  $PIXSE,DVLBI,±XXXXX,±YYYYY,±ZZZZZ,DDDD.DD,S*hh
                    if (fmLen == 7 && fmItems[0] == "$PIXSE" && fmItems[1] == "DVLBI")
                    {
                        double d1 = double.Parse(fmItems[2]);
                        double d2 = double.Parse(fmItems[3]);
                        double d3 = double.Parse(fmItems[4]);
                        double d4 = double.Parse(fmItems[5]);
                        int d5 = 0;
                        if (fmItems[6].Length > 0 && fmItems[6][0] == 'A')
                            d5 = 1;
                        else if (fmItems[6].Length > 0 && fmItems[6][0] == 'V')
                            d5 = 0;

                        DataFactory.SetVal(RegNameList[21], d2);
                        DataFactory.SetVal(RegNameList[22], d1);
                        DataFactory.SetVal(RegNameList[23], d3);
                        DataFactory.SetVal(RegNameList[24], d4);
                        DataFactory.SetVal(RegNameList[25], d5);
                        continue;
                    }

                    // --- $PIXSE,DVLWI,±XXXXX,±YYYYY,±ZZZZZ,DDDD.DD,S*hh
                    if (fmLen == 7 && fmItems[0] == "$PIXSE" && fmItems[1] == "DVLWI")
                    {
                        double d1 = double.Parse(fmItems[2]);
                        double d2 = double.Parse(fmItems[3]);
                        double d3 = double.Parse(fmItems[4]);
                        double d4 = double.Parse(fmItems[5]);
                        int d5 = 0;
                        if (fmItems[6].Length > 0 && fmItems[6][0] == 'A')
                            d5 = 1;
                        else if (fmItems[6].Length > 0 && fmItems[6][0] == 'V')
                            d5 = 0;

                        DataFactory.SetVal(RegNameList[26], d2);
                        DataFactory.SetVal(RegNameList[27], d1);
                        DataFactory.SetVal(RegNameList[28], d3);
                        DataFactory.SetVal(RegNameList[29], d4);
                        DataFactory.SetVal(RegNameList[30], d5);
                        continue;
                    }
                }
            }
            catch (Exception e)
            {
                return false;
            }                
            return true;
        }

        /// <summary>
        /// 产生发送帧
        /// </summary>
        /// <param name="NeedRecvLen"></param>
        /// <returns></returns>
        public override byte[] GenerateWriteFrame(out int NeedRecvLen)
        {
            var DataFactory = RegFactory.Instance;

            startTicks = Environment.TickCount;
            NeedRecvLen = 50;
            bool IsDockCal = false;
            while (writeQueue.Count > 0)
            {
                if (writeQueue.TryDequeue(out AddressAndVal data))
                {
                    if (data.Address == 0 &&(data.Value == 1 || data.Value == 2 || data.Value == 3 || data.Value == 9))   // 码头校准
                    {
                        double fLon = DataFactory.Regs[RegNameList[34]].dVal;
                        double fLat = DataFactory.Regs[RegNameList[35]].dVal;

                        string sTX_temp = $"PIXSE,SETPOS,{data.Value},{fLat:0.000000},{fLon:0.000000}";
                        //string sTX_temp = $"PIXSE,SETPOS,{data.Value},18.21116211,109.47442211";
                        byte[] bTx_temp = Encoding.UTF8.GetBytes(sTX_temp);
                        byte checksum = 0;
                        foreach (byte x in bTx_temp)
                        {
                            checksum ^= x;
                        }
                        string sTX_Frame = $"${sTX_temp}*{checksum:X2}\r\n";
                        byte[] bTx_Frame = Encoding.UTF8.GetBytes(sTX_Frame);
                        Tx_w = bTx_Frame;
                        IsDockCal = true;
                        continue;
                    }

                    if (data.Address == 1)   // 海上校准命令
                    {
                        IsSeaCal = (data.Value == 1);
                        IsFixPos = (data.Value == 2);
                        DataFactory.SetVal(RegNameList[33], data.Value);
                    }
                }
            }
            WriteFlag = false;
            if (IsDockCal)
            {
                return Tx_w;
            }
            else {
                return null;
            }
            
        }

        /// <summary>
        /// 处理写入帧的返回数据
        /// </summary>
        /// <param name="buffer"></param>
        /// <returns></returns>
        public override bool ProcessWriteFrame(byte[] buffer)
        {
           // WriteFlag = false;
            return ProcessReadFrame(buffer);
        }

        /// <summary>
        /// 改变变量的值
        /// </summary>
        /// <param name="Index"></param>
        /// <param name="Value"></param>
        public override void PushRegValue(int Index, double Value)
        {
            Index -= 32;  //寄存器存储顺序从0开始，先是只读存储器，后面是读写存储器

            if (Index >= 0 && (Index < 2))
            {
                writeQueue.Enqueue(new AddressAndVal(Index, Value));
                WriteFlag = true;
                NeedInit = false;
            }
            else if (Index >= 2 && Index < 10)
            {
                RegFactory.Instance.SetVal(RegNameList[32 + Index], Value);
            }
        }
    }
}
